// $Id: kalman_mobile.cpp 5925 2006-03-14 21:23:49Z tdelaet $
// Copyright (C) 2006 Tinne De Laet <first dot last at mech dot kuleuven dot be>
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation; either version 2.1 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA.

/* Demonstration program for the Bayesian Filtering Library.
   Mobile robot localization with respect to wall with different possibilities for filter
*/


// 文件开头部分包含了将要使用的BFL库头文件的包含语句。
#include <filter/extendedkalmanfilter.h>

#include <model/linearanalyticsystemmodel_gaussianuncertainty.h>
#include <model/linearanalyticmeasurementmodel_gaussianuncertainty.h>

#include <pdf/analyticconditionalgaussian.h>
#include <pdf/linearanalyticconditionalgaussian.h>

//移动机器人仿真器
#include "../mobile_robot.h"

#include <iostream>
#include <fstream>

// Include file with properties
// 从中读取某些属性（例如初始状态估计值）的文件
#include "../mobile_robot_wall_cts.h"

//以及我们将要使用的命名空间
using namespace MatrixWrapper;
using namespace BFL;
using namespace std;



/* The purpose of this program is to construct a kalman filter for the problem
  of localisation of a mobile robot equipped with an ultrasonic sensor.
  In this case the orientation is known, which simplifies the model considerably:
  The system model will become linear.
  The ultrasonic measures the distance to the wall (it can be switched off:
  see mobile_robot_wall_cts.h)

  The necessary SYSTEM MODEL is:

  x_k      = x_{k-1} + v_{k-1} * cos(theta) * delta_t
  y_k      = y_{k-1} + v_{k-1} * sin(theta) * delta_t

  The used MEASUREMENT MODEL:
  measuring the (perpendicular) distance z to the wall y = ax + b

  set WALL_CT = 1/sqrt(pow(a,2) + 1)
  z = WALL_CT * a * x - WALL_CT * y + WALL_CT * b + GAUSSIAN_NOISE
  or Z = H * X_k + J * U_k

  where

  H = [ WALL_CT * a       - WALL_CT      0 ]
  and GAUSSIAN_NOISE = N((WALL_CT * b), SIGMA_MEAS_NOISE)

*/


int main(int argc, char** argv)
{
  cerr << "==================================================" << endl
       << "Test of kalman filter" << endl
       << "Mobile robot localisation example" << endl
       << "==================================================" << endl;


  // 现在我们将构建系统模型，用于预测移动机器人在每个时间步的位置。该预测基于移动机器人的速度。
  // 由于移动机器人无法转向，位置预测将采用线性方程表示。鉴于该模型是线性的，BFL库已内置可直接使用的实现方案。

  /****************************
   *       线性系统模型         *
   ***************************/

  // 首先，我们构建一个线性模型，用于计算移动机器人在下一时间步的位置
  // 该计算基于机器人当前的位置以及其速度 ——这个速度由平移速度和旋转速度
  // 构成（在本首个示例中，旋转速度为零）。该线性模型定义如下：x_{k+1} = A * x_k + B * u_k
  // 我们构建 A 和 B 矩阵，A 矩阵用于描述状态转移，而 B 矩阵则用于描述输入对状态的影响。
  Matrix A(2,2);
  A(1,1) = 1.0;
  A(1,2) = 0.0;
  A(2,1) = 0.0;
  A(2,2) = 1.0;
  Matrix B(2,2);
  B(1,1) = cos(0.8);
  B(1,2) = 0.0;
  B(2,1) = sin(0.8);
  B(2,2) = 0.0;

  vector<Matrix> AB(2);
  AB[0] = A;
  AB[1] = B;

  // 随后，我们创建一个高斯分布——该分布由均值（Mu）和协方差（Cov）定义。
  // 此高斯分布用于表征移动机器人预测位置所具有的不确定性。
  // 其中，均值由 MU_SYSTEM_NOISE_X 和 MU_SYSTEM_NOISE_Y 定义。
  ColumnVector sysNoise_Mu(2);
  sysNoise_Mu(1) = MU_SYSTEM_NOISE_X;
  sysNoise_Mu(2) = MU_SYSTEM_NOISE_Y;

  // 协方差矩阵设为对角矩阵，其对角线元素分别为 SIGMA_SYSTEM_NOISE_X 和 SIGMA_SYSTEM_NOISE_Y。
  SymmetricMatrix sysNoise_Cov(2);
  sysNoise_Cov = 0.0;
  sysNoise_Cov(1,1) = SIGMA_SYSTEM_NOISE_X;
  sysNoise_Cov(1,2) = 0.0;
  sysNoise_Cov(2,1) = 0.0;
  sysNoise_Cov(2,2) = SIGMA_SYSTEM_NOISE_Y;

  // 均值与协方差共同定义了一个二维高斯分布：
  Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov);

  // 现在，我们创建一个线性条件概率密度函数（PDF），用于表示在已知移动机器人当前位置的情况下，其预测位置出现的概率。
  // 该概率密度函数由向量 AB（代表线性模型）和表示系统模型额外不确定性的高斯分布共同定义。
  LinearAnalyticConditionalGaussian sys_pdf(AB, system_Uncertainty);
  
  // 最后，我们根据系统概率密度函数（PDF）创建系统模型。
  LinearAnalyticSystemModelGaussianUncertainty sys_model(&sys_pdf);

  /*********************************
   *        初始化测量模型           *
   ********************************/

  // 现在我们将构建测量模型，用于根据机器人到墙壁的距离测量值来修正移动机器人位置的预测结果。由于本示例采用线性测量模型，BFL库已内置可直接使用的实现方案。
  // 首先，我们构建一个线性模型，该模型将距离测量值与移动机器人的位置相关联。
  // 该模型定义为：z_{k+1} = H * x_{k+1}
  // 构建 H 矩阵
  Matrix H(1,2);
  double wall_ct = 2/(sqrt(pow(RICO_WALL,2.0) + 1));
  H = 0.0;
  H(1,1) = wall_ct * RICO_WALL;
  H(1,2) = 0 - wall_ct;

  // 其中，wall_ct 是一个辅助常量，RICO_WALL 表示墙壁的斜率。
  // 接着，我们创建一个高斯分布——该分布由均值（Mu）和协方差（Cov）定义，
  // 用于表征测距到墙壁时所产生的测量不确定性。此处的均值设定为 MU_MEAS_NOISE。

  // Construct the measurement noise (a scalar in this case)
  ColumnVector measNoise_Mu(1);
  measNoise_Mu(1) = MU_MEAS_NOISE;

  // 协方差矩阵为对角矩阵，其方差参数 SIGMA_MEAS_NOISE 表示 \sigma^2 的边界值。
  SymmetricMatrix measNoise_Cov(1);
  measNoise_Cov(1,1) = SIGMA_MEAS_NOISE;

  // 均值与协方差共同定义了一个二维高斯分布：
  Gaussian measurement_Uncertainty(measNoise_Mu, measNoise_Cov);

  // 现在，我们创建一个线性概率密度函数（PDF），用于表示距离测量的概率分布。
  // 该概率密度函数由矩阵 H（代表线性模型）和描述测量不确定性的高斯分布共同定义
  LinearAnalyticConditionalGaussian meas_pdf(H, measurement_Uncertainty);

  //最后，我们根据测量概率密度函数（PDF）创建测量模型。
  LinearAnalyticMeasurementModelGaussianUncertainty meas_model(&meas_pdf);


  /****************************
   *      线性先验分布          *
   ***************************/

  // 现在，我们将构建先验分布，用于表示初始状态估计值以及该初始估计值所具有的不确定性。
  // 该先验分布是一个高斯分布，其均值（prior_Mu）和协方差（prior_Cov）分别定义了分布的参数。其中，均值即为初始状态估计值。
  ColumnVector prior_Mu(2);
  prior_Mu(1) = PRIOR_MU_X;
  prior_Mu(2) = PRIOR_MU_Y;
  // 协方差是一个对角矩阵，其中 PRIOR_COV_X和 PRIOR_COV_Y表示 /sigma^2 的边界值。
  SymmetricMatrix prior_Cov(2);
  prior_Cov(1,1) = PRIOR_COV_X;
  prior_Cov(1,2) = 0.0;
  prior_Cov(2,1) = 0.0;
  prior_Cov(2,2) = PRIOR_COV_Y;
  // 均值与协方差共同定义了一个二维高斯分布：
  Gaussian prior(prior_Mu,prior_Cov);


  /******************************
   * Construction of the Filter *
   ******************************/
  // 在本示例中，我们将使用卡尔曼滤波器来估计移动机器人的未知位置。为此，BFL库提供了 ExtendedKalmanFilter 类来实现这一功能。
  // 向 ExtendedKalmanFilter 类构造函数传入的唯一参数就是先验分布
  ExtendedKalmanFilter filter(&prior);


  /***************************
   * initialise MOBILE ROBOT *
   **************************/
  // 创建了一个名为 MobileRobot 的移动机器人仿真器，用于模拟机器人在包含一面墙的虚拟环境中行驶的场景。
  // 在实际物理实验中，该仿真器会被真实的移动机器人所替代。
  // 移动机器人的各项属性（例如初始位置、系统仿真噪声等）均在配置文件 mobile_robot_wall.cfg 中定义。
  // 移动机器人仿真器的创建方式如下：
  MobileRobot mobile_robot;
  ColumnVector input(2);
  input(1) = 0.1;
  input(2) = 0.0;

  /*******************
   * ESTIMATION LOOP *
   *******************/
  cout << "MAIN: Starting estimation" << endl;
  unsigned int time_step;
  for (time_step = 0; time_step < NUM_TIME_STEPS-1; time_step++)
    {
      // 该方法以指定的速度输入（input）驱动移动机器人运动1秒钟。
      // 在每个时间步长内，移动机器人必须接收速度输入指令以实现预期的运动控制。
      mobile_robot.Move(input);

      // 该方法返回移动机器人当前位置到墙壁的单次距离测量值。
      // 当移动机器人到达新位置时，系统会测量其与墙壁之间的距离：
      ColumnVector measurement = mobile_robot.Measure();

      // 在每个时间步，可以基于系统模型和测量模型提供的信息生成新的状态估计。
      // 若要利用测量信息和系统模型信息来更新滤波器，其更新操作如下：
      filter.Update(&sys_model,input,&meas_model,measurement);
    } // estimation loop


  // 要获取经更新后滤波器的后验分布（该分布综合了系统模型与测量信息的全部结果），可通过以下方式：
  Pdf<ColumnVector> * posterior = filter.PostGet();

  // 后验分布的期望值（均值）和协方差矩阵可通过以下接口获得：

  cout << "After " << time_step+1 << " timesteps " << endl;
  cout << " Posterior Mean = " << endl << posterior->ExpectedValueGet() << endl
       << " Covariance = " << endl << posterior->CovarianceGet() << "" << endl;


  cout << "======================================================" << endl
       << "End of the Kalman filter for mobile robot localisation" << endl
       << "======================================================"
       << endl;


  return 0;
}
